{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 相机校准"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 导入头文件"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/usr/local/lib/python3.10/dist-packages/torchvision/io/image.py:13: UserWarning: Failed to load image Python extension: \n",
      "  warn(f\"Failed to load image Python extension: {e}\")\n"
     ]
    }
   ],
   "source": [
    "#!/usr/bin/env python\n",
    "# coding: utf-8\n",
    "import sys\n",
    "import os\n",
    "\n",
    "sys.path.append(\"../dofbot_garbage_yolov5/utils\")\n",
    "import Arm_Lib\n",
    "import cv2 as cv\n",
    "import threading\n",
    "from time import sleep\n",
    "from dofbot_config import ArmCalibration, read_XYT, write_XYT\n",
    "import ipywidgets as widgets\n",
    "from IPython.display import display\n",
    "from garbage_identify import GarbageIdentify"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 创建实例,初始化参数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "y_offset is 0.016\n",
      "x_offset is 0.01\n",
      "finish init..\n",
      "read xy is: [88, 131]\n"
     ]
    }
   ],
   "source": [
    "# 创建获取目标实例\n",
    "target = GarbageIdentify(test_mode=True)\n",
    "# 创建相机标定实例\n",
    "calibration = ArmCalibration()\n",
    "# 创建机械臂驱动实例\n",
    "arm = Arm_Lib.Arm_Device()\n",
    "# 初始化一些参数\n",
    "num=0\n",
    "# 初始化标定方框边点\n",
    "dp = []\n",
    "# 初始化抓取信息\n",
    "msg = {}\n",
    "# 初始化1,2舵机角度值\n",
    "xy = [90, 135]\n",
    "# 初始化二值图阈值\n",
    "threshold = 140\n",
    "# 初始化模式\n",
    "model = \"General\"\n",
    "# XYT参数路径\n",
    "pkg_parent_dir = os.path.dirname(os.path.dirname(os.path.realpath(\"__file__\")))\n",
    "pkg_dir = os.path.join(pkg_parent_dir, \"dofbot_garbage_yolov5\")\n",
    "cfg_dir = os.path.join(pkg_dir, \"config\")\n",
    "XYT_cfg_dir = os.path.join(cfg_dir, \"XYT_config.txt\")\n",
    "data_idx_cfg_dir = os.path.join(cfg_dir, \"data_collect_idx.txt\")\n",
    "dp_cfg_dir = os.path.join(cfg_dir, \"dp.bin\")\n",
    "XYT_path = XYT_cfg_dir\n",
    "try: \n",
    "    xy, thresh = read_XYT(XYT_path)\n",
    "    print(\"read xy is:\", xy)\n",
    "except Exception: \n",
    "    print(\"Read XYT_config Error !!!\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 初始化机械臂位置"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import Arm_Lib\n",
    "arm = Arm_Lib.Arm_Device()\n",
    "joints_0 = [xy[0], xy[1], 0, 0, 90, 30] \n",
    "arm.Arm_serial_servo_write6_array(joints_0, 1000)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 创建控件"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "button_layout      = widgets.Layout(width='320px', height='60px', align_self='center')\n",
    "output = widgets.Output()\n",
    "# 调整滑杆\n",
    "joint1_slider      = widgets.IntSlider(description='joint1 :'   ,    value=xy[0]     , min=70 , max=110, step=1, orientation='horizontal')\n",
    "joint2_slider      = widgets.IntSlider(description='joint2 :'   ,    value=xy[1]     , min=50, max=155, step=1, orientation='horizontal') # REVISE\n",
    "threshold_slider   = widgets.IntSlider(description='threshold :',    value=threshold , min=0  , max=255, step=1, orientation='horizontal')\n",
    "\n",
    "# 进入标定模式\n",
    "calibration_model  = widgets.Button(description='calibration_model',  button_style='primary', layout=button_layout)\n",
    "calibration_ok     = widgets.Button(description='calibration_ok',     button_style='success', layout=button_layout)\n",
    "calibration_cancel = widgets.Button(description='calibration_cancel', button_style='danger', layout=button_layout)\n",
    "\n",
    "# 退出\n",
    "exit_button = widgets.Button(description='Exit', button_style='danger', layout=button_layout)\n",
    "\n",
    "# 添加布局\n",
    "imgbox = widgets.Image(format='jpg', height=480, width=640, layout=widgets.Layout(align_self='center'))\n",
    "garbage_identify = widgets.VBox(\n",
    "    [joint1_slider, joint2_slider, threshold_slider, calibration_model, calibration_ok, calibration_cancel, exit_button],\n",
    "    layout=widgets.Layout(align_self='center'));\n",
    "controls_box = widgets.HBox([imgbox, garbage_identify], layout=widgets.Layout(align_self='center'))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 标定回调"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 标定回调\n",
    "def calibration_model_Callback(value):\n",
    "    global model\n",
    "    model = 'Calibration'\n",
    "    with output: \n",
    "        print(model)\n",
    "def calibration_OK_Callback(value):\n",
    "    global model\n",
    "    model = 'calibration_OK'\n",
    "    with output: \n",
    "        print(model)\n",
    "def calibration_cancel_Callback(value):\n",
    "    global model\n",
    "    model = 'calibration_Cancel'\n",
    "    with output: \n",
    "        print(model)\n",
    "        \n",
    "calibration_model.on_click(calibration_model_Callback)\n",
    "calibration_ok.on_click(calibration_OK_Callback)\n",
    "calibration_cancel.on_click(calibration_cancel_Callback)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def exit_button_Callback(value):\n",
    "    global model\n",
    "    model = 'Exit'\n",
    "    with output: \n",
    "        print(model)\n",
    "        \n",
    "exit_button.on_click(exit_button_Callback)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 主程序"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def camera():\n",
    "    global model, dp, msg, HSV_name\n",
    "    # 打开摄像头\n",
    "    capture = cv.VideoCapture(0)\n",
    "    print(\"Cam has FPS:\", capture.get(cv.CAP_PROP_FPS))\n",
    "    # 当摄像头正常打开的情况下循环执行\n",
    "    while capture.isOpened():\n",
    "        try:\n",
    "            # 读取相机的每一帧\n",
    "            _, img = capture.read()\n",
    "            # 统一图像大小\n",
    "            img = cv.resize(img, (640, 480))\n",
    "            xy = [joint1_slider.value, joint2_slider.value]\n",
    "            if model == 'Calibration':\n",
    "                _, img = calibration.calibration_map(img, xy, threshold_slider.value)\n",
    "            if model == 'calibration_OK':\n",
    "                try: \n",
    "                    print(\"Try to write dp..\")\n",
    "                    write_XYT(XYT_path, xy, threshold_slider.value)\n",
    "                    print(\"Finish Write dp..\")\n",
    "                except Exception: \n",
    "                    print(\"File XYT_config Error !!!\")\n",
    "                dp, img = calibration.calibration_map(img, xy, threshold_slider.value)\n",
    "                print(\"dp is\", dp)\n",
    "                dp.tofile(dp_cfg_dir)\n",
    "                print(\"saved bin..\")\n",
    "                model=\"General\"\n",
    "            if len(dp) != 0: \n",
    "                img = calibration.perspective_transform(dp, img)\n",
    "            if model == 'calibration_Cancel':  \n",
    "                dp = []\n",
    "                msg= {}\n",
    "                model=\"General\"\n",
    "                \n",
    "            if model == 'Exit':\n",
    "                cv.destroyAllWindows()\n",
    "                capture.release()\n",
    "                break\n",
    "            imgbox.value = cv.imencode('.jpg', img)[1].tobytes()\n",
    "        except KeyboardInterrupt:capture.release()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 启动"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a0d68c94a4b245b39ba98bdb3a5560b0",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(Image(value=b'', format='jpg', height='480', layout=\"Layout(align_self='center')\", width='640')…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "1de2ab9895b34c6981b8953a4ea13b19",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Output()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Cam has FPS: 30.0\n",
      "Try to write dp..\n",
      "Finish Write dp..\n",
      "dp is [[ 75  35]\n",
      " [ 14 452]\n",
      " [627 436]\n",
      " [540  22]]\n",
      "saved bin..\n"
     ]
    }
   ],
   "source": [
    "display(controls_box,output)\n",
    "threading.Thread(target=camera, ).start()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  },
  "vscode": {
   "interpreter": {
    "hash": "916dbcbb3f70747c44a77c7bcd40155683ae19c65e1c03b4aa3499c5328201f1"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
